#!/usr/bin/python
# -*- coding: UTF-8 -*-


import time,threading,sys
sys.path.append("/home/biowin/Minibot/04.RobotSamples/03.SpiderRobot")
from BwRobotLib import BwRobotLib
from SpiderRobot import SpiderRobot
from BwSensorLib import BwSensorLib
import os,inspect

# 加载机器人构型配置
fileName = inspect.getframeinfo(inspect.currentframe()).filename
folderPath     = os.path.dirname(os.path.abspath(fileName))
cfile=folderPath+"/config.txt"

class FollowRobot:
	def __init__(self):
		# 实例化传感器库对象
		self.sensorlib = BwsensorliblibLib("udp")
		# 实例化BwRobotLib库
		self.robotlib = BwRobotLib("udp")
		# 实例化蜘蛛机器人库
		self.spider = SpiderRobot(self.robotlib)
		# 连接机器人设备
		self.robotlib.connectRobot("10.10.100.254",8899) #真机AP模式
        # 导入机器人配置参数
        self.robotlib.loadConfig(config_file)
		#配置ID
		self.spider.bindIds(self.robotlib.module_ids)
		# 时间变量
		self.wait_time = 0.5
		# 蜘蛛机器人运动标志位
		self.flag_threading = 0 
		# 点阵显示
		#　向前箭头 向后箭头　
		self.display_data = [[0x08,0x1C,0x2A,0x08,0x08],[0x08,0x08,0x2A,0x1C,0x08]]
	
	def followRobotMotion(self):
		while 1:
			dcm = self.sensorlib.readUltrasonicSensor() # 读取超声波传感器数据
			print(dcm)
			# 大于20cm
			if (dcm >= 20):
				self.sensorlib.setMatrixSensor(self.displayData[0]) # 点阵显示向前箭头
				self.spider.goForward() # 蜘蛛机器人向前走
			# 小于20cm	
			else:
				self.sensorlib.setMatrixSensor(self.displayData[1]) # 点阵显示向后箭头
				self.spider.goBack() # 蜘蛛机器人向后走

			
# 主函数
def main():
    # 实例化跟随机器人类
    followRobot = FollowRobot()
    # 执行跟随机器人功能
    followRobot.followRobotMotion()


if __name__ == "__main__":
    try:
        main()
    except KeyboardInterrupt:
        print(" Program closed! ")
    pass
